#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include "swarm_avoid_client/global_functions.hpp"

using namespace GeographicLib;

void wgs84_to_enu(double lat, double lon, double alt, 
                  double lat_ref, double lon_ref, double alt_ref,
                  double& north, double& east, double& down) {
    // 初始化 LocalCartesian 对象，用参考点定义局部坐标系
    GeographicLib::LocalCartesian local(lat_ref, lon_ref, alt_ref);

    // 将 WGS84 坐标转换为 NED 坐标
    local.Forward(lat, lon, alt, north, east, down);
}

void enu_to_wgs84(double north, double east, double down, 
                  double lat_ref, double lon_ref, double alt_ref,
                  double& lat, double& lon, double& alt) {
    // 初始化 LocalCartesian 对象，用参考点定义局部坐标系
    GeographicLib::LocalCartesian local(lat_ref, lon_ref, alt_ref);

    // 将 NED 坐标转换为 WGS84 坐标
    local.Reverse(north, east, down, lat, lon, alt);
}